// Node name: "feedback_node.cpp".
// This is an a simple publisher which broadcasts joint position.
// Written by Ellipse in 5 Aug.

#include <string>
#include <ros/ros.h>
#include <time.h>
#include "scservo/SCServo.h"
#include "std_msgs/Float64MultiArray.h"
#include "Motor_parameter.h"



SMSBL s;

int main(int argc, char** argv)
{
    s16 currentpos[IDN] = {0, 0, 0, 0, 0, 0};
	// Define a standard message and add 6 elements in it.
	std_msgs::Float64MultiArray msg;
	msg.data.push_back(0);
	msg.data.push_back(0);
	msg.data.push_back(0);
	msg.data.push_back(0);
	msg.data.push_back(0);
	msg.data.push_back(0);

	// Initialize the node.
	ros::init(argc, argv, "fobot_joint_position_feedback");

    // Create a handle to the processing node.
	ros::NodeHandle n;

	// Initialize a publisher object.
	ros::Publisher pos_pub = n.advertise<std_msgs::Float64MultiArray>("/fobot_joint_position_publisher", 100);

    // Specify a frequency that you would like to loop at.
	ros::Rate loop_rate(50);

    //open serial
    if(s.begin(1000000, "/dev/ttyUSB0"))
    {
        ROS_INFO("Port \"dev/ttyUSB0\" is opened!");
    }
    else
    {
	ROS_INFO("Cannot open the port!");
	return 1;
    }

	while(ros::ok())
	{
		//Read current joint position
		for (int i = 0; i < IDN; i++)
		{
		    if(s.FeedBack(i)!=-1){
                        if(i == 2)
                        {
                            currentpos[2] = (s16)((s.ReadPos(1) + s.ReadPos(2) - BiasPos[2])/SMSPOSTRANS);
                            std::cout<<"ok"<<i<<std::endl;

                        }
                        else
                        {
                            currentpos[i] = (s16)((s.ReadPos(i) - BiasPos[i])/SMSPOSTRANS/Dirction[i]);
                            std::cout<<"ok"<<i<<std::endl;
                        }
		     }
   		     else{
			std::cout<<"Read Err ="<<i<<std::endl;
		    }
		}
        	//push current joint position in message
        	for (int i = 0; i < IDN; i++)
        	{
            	    msg.data.push_back(currentpos[i]);
        	}
		// Broadcast the message to anyone who is connected.
		pos_pub.publish(msg);

		// It is not necessary for this simple program.
		// But add it for good measure.
		ros::spinOnce();

		// Sleep for a certain amount of time.
		loop_rate.sleep();
	}

	return 0;
}

